#include "boocaxsocket.h"
#include "../network_common.h"

BoocaxSocket::BoocaxSocket(QTcpSocket *parent) :
    QTcpSocket(parent)
{
    setSocketOption(QAbstractSocket::LowDelayOption, "1");
    setSocketOption(QAbstractSocket::KeepAliveOption, "1");

    //this->hostAddress = new QHostAddress("127.0.0.1");
    this->hostAddress = new QHostAddress("192.168.31.41");
    this->port = 6789;
    this->readByteCount = 0;

    connect(this, &QTcpSocket::readyRead, this, &BoocaxSocket::readJsonData);
}

BoocaxSocket::~BoocaxSocket()
{
    close();
    delete this->hostAddress;
}

bool BoocaxSocket::connectToBoocax()
{
    bool isConnectToBoocax = true;
    connectToHost(*this->hostAddress, port);
    bool isConnected =  waitForConnected(3000);
    if(!isConnected)    isConnectToBoocax = false;
    else {
        //注册客户端
        QJsonObject reg_client_message_obj;
        reg_client_message_obj.insert("message_type", "register_client");
        reg_client_message_obj.insert("client_type", 3);
        reg_client_message_obj.insert("mac_address", "host_mac");
        this->writeJsonData(reg_client_message_obj);
        qDebug()<<getHostMacAddress();
    }
    return isConnectToBoocax;
}

void BoocaxSocket::readJsonData()
{
    while (bytesAvailable())
    {
        if(!this->readByteCount)
        {
            char len[4];
            read(len, sizeof(len));
            //计算消息的字节长度
            this->readByteCount = len[0] & 0x000000FF;
            this->readByteCount |= ((len[1] << 8) & 0x0000FF00);
            this->readByteCount |= ((len[2] << 16) & 0x00FF0000);
            this->readByteCount |= ((len[3] << 24) & 0xFF000000);
        }
        if(this->readByteCount<0)
        {
            QByteArray jsonMs = readAll();
            return;
        }
        if(this->readByteCount > bytesAvailable())
        {
            return;
        }
        QByteArray jsonMessage;
        jsonMessage.resize(this->readByteCount);
        jsonMessage = read(jsonMessage.size());
        this->readByteCount = 0;
        QJsonParseError parseError;
        QJsonDocument jsonDoc(QJsonDocument::fromJson(jsonMessage,&parseError));
        if(parseError.error == QJsonParseError::NoError)
        {
            QJsonObject json_obj = jsonDoc.object();
            this->parserJsonData(json_obj);
        }
        else
        {
            qDebug()<<parseError.errorString();
        }
    }
}

void BoocaxSocket::writeJsonData(QJsonObject &obj)
{
    QByteArray array;
    QJsonDocument jsonDoc;
    jsonDoc.setObject(obj);
    array.prepend(jsonDoc.toJson(QJsonDocument::Compact));

    int lenInt = array.length();
    char len[4];
    len[0] = (char)(lenInt & 0xff);
    len[1] = (char)((lenInt >> 8) & 0xff);
    len[2] = (char)((lenInt >> 16) & 0xff);
    len[3] = (char)((lenInt >> 24) & 0xff);
    array.prepend(len,sizeof(len));

    write(array);
}

void BoocaxSocket::parserJsonData(QJsonObject &jsonObj)
{
    QString message_type = jsonObj.value("message_type").toString();
    if(message_type == "all_robot_info")
    {

    }
    else if(message_type == "report_pos_vel_status")
    {
        QJsonObject pose = jsonObj.value("pose").toObject();
        setPoseX(pose.value("x").toDouble());
        setPoseY(pose.value("y").toDouble());
        setPoseYaw(pose.value("yaw").toDouble());
        QJsonObject vel = jsonObj.value("vel").toObject();
        setVelVx(vel.value("vx").toDouble());
        setVelVy(vel.value("vy").toDouble());
        setVelVtheta(vel.value("vtheta").toDouble());
        emit reportPosVelStatusChanged();
    }
    else if(message_type == "report_basic_status")
    {
        QJsonObject basic_status = jsonObj.value("basic_status").toObject();
        setUwb(basic_status.value("uwb").toInt());
        setLaser(basic_status.value("laser").toInt());
        setBase(basic_status.value("base").toInt());
        setOdom(basic_status.value("odom").toInt());
        emit basicStatusChanged();
    }
    else if(message_type == "report_move_status")
    {
        setMoveStatus(jsonObj.value("move_status").toInt());
    }
    else if(message_type == "report_loc_status")
    {
        setLocStatus(jsonObj.value("loc_status").toInt());
    }
    //else if(message_type = "")
}
